#include "featureObjectExtractor.h"
#include "LSfeatureObject.h"
#include "stdlib.h"
#include <ros/ros.h>
featureObjectExtractor::featureObjectExtractor()
{

}
vector <LSfeatureObject> featureObjectExtractor::doExtract(vector<objectPoint> p, double MaxLength , double minLength)
{

    vector <LSfeatureObject>  lst;
    int startObjIndex, obj = 0, i = 0;
    LSfeatureObject  *sObj;
    LSfeatureObject lastObj;
    while ( i < p.size() )
    {
        startObjIndex = i;
        //integration phase 1
        for ( ++i; i < p.size(); i++ )
            if ( fabs( p[i].getR() - p[i - 1].getR() ) > 10)
                break;
        if( startObjIndex == i - 1 )
            continue;

        sObj = new LSfeatureObject( p , startObjIndex , i - 1 , ++obj);

        if (( sObj->getLength() >= MaxLength ))
        {

            delete sObj;
            continue;
        }
        if (lst.size() > 0)
        {
            lastObj = lst.at( lst.size() - 1 );
            //integration phase 2
            if ( lastObj.getEndPoint().getDistanceFromPoint( sObj->getStartPoint() ) < 15)
            {
                vector<LSfeatureObject>::iterator ii = lst.begin();
                *sObj += lastObj;
                if (( sObj->getLength() >= MaxLength ))
                {

                    delete sObj;
                    continue;
                }
                if ((sObj->getLength() <= minLength ))
                {
                    continue;

                }
                double digonal = sObj->getDigonalBoundingBox();
                if( (MinDgl < digonal) && (MaxDgl > digonal) )
                    if((sObj->getWidth()>=0)&& (sObj->getWidth()<=30))
                    {
                        advance(ii,lst.size()-1);
                        ii = lst.erase(ii);
                        lastObj.confidenceFactor =  getStandardDeviation(digonal);
                        //cout<<"\n exp : " <<lastObj.confidenceFactor;
                        lst.push_back(lastObj);
                        delete sObj;
                        continue;
                    }
            }
        }

        double digonal = sObj->getDigonalBoundingBox();
        if( (MinDgl <= digonal)&& (MaxDgl > digonal))
        {
          //  cout<<"\n sObj->getWidth : "<<sObj->getWidth();
            if((sObj->getWidth()>=0)&& (sObj->getWidth()<=30))
            {
                sObj->confidenceFactor = getStandardDeviation(digonal) ;
                lst.push_back(*sObj);
            }
        }
        delete sObj;
    }
    return lst;
}
double featureObjectExtractor::getStandardDeviation(double x)
{
    double med = (MinDgl + MaxDgl) / 2;
    double delmed = MaxDgl - med;
    return exp(- fabs( x - delmed ) / med ) ;
}

void featureObjectExtractor::setDiagonal(double max, double min)
{
    MaxDgl = max;
    MinDgl = min;
}
